// --------------------------------------------------------------------------------------------------------------------
// <copyright file="GyroscopeState.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Hardware.DAC
{
    using System.Runtime.Serialization;

    using Microsoft.Robotics.Algorithms;
    using Microsoft.Robotics.Numerics;

    /// <summary>
    /// The Gyroscope sensor state class
    /// </summary>
    [DataContract]
    public class GyroscopeState : IDeviceState
    {
        /// <summary>
        /// Initializes a new instance of the <see cref="GyroscopeState"/> class.
        /// </summary>
        public GyroscopeState()
        {
        }

        /// <summary>
        /// Initializes a new instance of the <see cref="GyroscopeState"/> class.
        /// </summary>
        /// <param name="other">Copy constructor param</param>
        public GyroscopeState(GyroscopeState other)
        {
            this.DeviceId = other.DeviceId;
            this.DeviceType = other.DeviceType;
            this.Model = other.Model;
            this.Pose = other.Pose;
            this.RotationalVelocityInRadiansPerSecond = other.RotationalVelocityInRadiansPerSecond;
            this.TimestampInMilliseconds = other.TimestampInMilliseconds;
            this.TimestampInTicks = other.TimestampInTicks;
        }

        /// <summary>
        /// Gets or sets the radians/sec vector for the gyroscope sensor
        /// </summary>
        [DataMember]
        public Vector3 RotationalVelocityInRadiansPerSecond { get; set; }

        /// <summary>
        /// Gets or sets the timestamp for the state.
        /// </summary>
        [DataMember]
        public long TimestampInTicks { get; set; }

        /// <summary>
        /// Gets or sets the timestamp in milliseconds (as opposed to hardware-dependent ticks).
        /// </summary>
        [DataMember]
        public long TimestampInMilliseconds { get; set; }

        /// <summary>
        /// Gets or sets the device identifier
        /// </summary>
        [DataMember]
        public int DeviceId { get; set; }

        /// <summary>
        /// Gets or sets the device model information
        /// </summary>
        [DataMember]
        public string Model { get; set; }

        /// <summary>
        /// Gets or sets the type of the device the sensor is related
        /// </summary>
        [DataMember]
        public DeviceType DeviceType { get; set; }

        /// <summary>
        /// Gets or sets the pose for the device/sensor
        /// </summary>
        [DataMember]
        public Pose Pose { get; set; }

        /// <summary>
        /// Interpolates the between the start and end states for the desiredTime.
        /// </summary>
        /// <param name="startTime">The start time for interpolation</param>
        /// <param name="endTime">The end time for interpolation</param>
        /// <param name="desiredTime">Desired time for interpolation. This must be between (inclusive) start and end times</param>
        /// <param name="start">Start value</param>
        /// <param name="end">End value</param>    
        /// <returns>The linear interpolated value at the desired time</returns>
        public static GyroscopeState Interpolate(long startTime, long endTime, long desiredTime, GyroscopeState start, GyroscopeState end)
        {
            GyroscopeState interpolatedState = new GyroscopeState(start);

            double interpolatedX = Pose2D.LinearInterpolateHeading(startTime, endTime, desiredTime, start.RotationalVelocityInRadiansPerSecond.X, end.RotationalVelocityInRadiansPerSecond.X);
            double interpolatedY = Pose2D.LinearInterpolateHeading(startTime, endTime, desiredTime, start.RotationalVelocityInRadiansPerSecond.Y, end.RotationalVelocityInRadiansPerSecond.Y);
            double interpolatedZ = Pose2D.LinearInterpolateHeading(startTime, endTime, desiredTime, start.RotationalVelocityInRadiansPerSecond.Z, end.RotationalVelocityInRadiansPerSecond.Z);
            interpolatedState.TimestampInMilliseconds = (long)Interpolator.LinearInterpolate(startTime, endTime, desiredTime, start.TimestampInMilliseconds, end.TimestampInMilliseconds);
            interpolatedState.TimestampInTicks = (long)Interpolator.LinearInterpolate(startTime, endTime, desiredTime, start.TimestampInTicks, end.TimestampInTicks);
            interpolatedState.RotationalVelocityInRadiansPerSecond = new Vector3(interpolatedX, interpolatedY, interpolatedZ);
            return interpolatedState;
        }
    }
}
